'''
Description: 
Version: V1.0
Author: zw_1520@163.com
Date: 2025-05-15 09:38:43
LastEditors: zw_1520@163.com
LastEditTime: 2025-05-17 08:11:22
All rights reserved.
'''
#!/usr/bin/env python3
import rclpy
from agibot_controller.actions import ensure_action, get_action
from rclpy.node import Node
from rclpy.qos import QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy
from sensor_msgs.msg import JointState
import time
import requests
import json
import base64


class ArmPublisher(Node):
    def __init__(self, arm_topic_name: str):
        super().__init__("arm_publisher")

        qos_profile = QoSProfile(
            history=QoSHistoryPolicy.KEEP_LAST, depth=10, reliability=QoSReliabilityPolicy.BEST_EFFORT
        )

        self.publisher_ = self.create_publisher(
            JointState, arm_topic_name, qos_profile)

        timer_period = 0.02
        # self.timer = self.create_timer(timer_period, self.timer_callback)
        # self.pos_ini = [0.0, 1.2, 0.0, -0.5, 1.5, 0.0,
        #                 0.0, 0.0, -1.2, 0.0, 0.5, 1.5, 0.0, 0.0]
        self.pos_ini = [0.0, 1.3, 0.0, -0.1, 1.57, 0.0,
                        0.0, 0.0, -1.3, 0.0, 0.1, 1.57, 0.0, 0.0]

        self.current_pos_offset = 0.0
        self.increasing = True

        self.move_count = 200
        self.step = 1.4 / self.move_count * (-1)
        self.bow_step = 1.57 / self.move_count * (-1)

    def position_control(self):
        msg = JointState()
        msg.header.stamp = self.get_clock().now().to_msg()

        current_position = list(self.pos_ini)

        for i in range(1, self.move_count):
            current_position[1] = self.pos_ini[1] + self.step
            self.pos_ini[1] = self.pos_ini[1] + self.step

            current_position[4] = self.pos_ini[4] + self.bow_step
            self.pos_ini[4] = self.pos_ini[4] + self.bow_step

            self.get_logger().info(
                f"position_control Publishing: current_position[1]: {current_position[1]}")
            msg.position = current_position

            self.publisher_.publish(msg)

            # Sleep for 0.02 seconds
            time.sleep(0.01)

    def position_recover(self):
        msg = JointState()
        msg.header.stamp = self.get_clock().now().to_msg()

        current_position = list(self.pos_ini)

        for i in range(1, self.move_count):
            current_position[1] = self.pos_ini[1] - self.step
            self.pos_ini[1] = self.pos_ini[1] - self.step

            current_position[4] = self.pos_ini[4] - self.bow_step
            self.pos_ini[4] = self.pos_ini[4] - self.bow_step

            self.get_logger().info(
                f"position_control Publishing: current_position[1]: {current_position[1]}")
            msg.position = current_position

            self.publisher_.publish(msg)

            # Sleep for 0.02 seconds
            time.sleep(0.01)

    def timer_callback(self):
        msg = JointState()
        msg.header.stamp = self.get_clock().now().to_msg()

        current_position = list(self.pos_ini)
        current_position[3] = self.pos_ini[3] + self.current_pos_offset
        current_position[10] = self.pos_ini[10] - self.current_pos_offset
        msg.position = current_position

        if self.increasing:
            self.current_pos_offset += self.step
            if self.current_pos_offset >= self.pos_offset_max:
                self.current_pos_offset = self.pos_offset_max
                self.increasing = False
        else:
            self.current_pos_offset -= self.step
            if self.current_pos_offset <= self.pos_offset_min:
                self.current_pos_offset = self.pos_offset_min
                self.increasing = True

        # self.publisher_.publish(msg)
        self.get_logger().info(
            f"Publishing: {{pos[3]: {current_position[3]:.2f}, pos[10]: {current_position[10]:.2f}}}"
        )


def encoded_text(text):
    text = text
    encoded_bytes = base64.b64encode(text.encode('utf-8'))
    encoded_text = encoded_bytes.decode('utf-8')

    return encoded_text


def stop_motion_data():
    """ Constructs stop motion data"""
    data = {
        "motion_id": "",
        "duration_ms": 10000,
        "cmd_end": True,
        "cmd_pause": True,
        "cmd_reset": True
    }
    return data


def stop_agi_motion_playing():
    """ Stops the motion playing"""
    current_action = get_action(mc_ip="192.168.100.100")
    allowed_actions = [
        "McAction_RL_LOCOMOTION_DEFAULT",
        "McAction_RL_LOCOMOTION_ARM_EXT_JOINT_SERVO",
        "McAction_RL_NAVIGATION_DEFAULT",
    ]
    if current_action not in allowed_actions:
        print(
            f"Current action: {current_action} is not allowed, please set to one of the following: {allowed_actions}")
        return
    if not ensure_action("McAction_RL_LOCOMOTION_ARM_EXT_JOINT_SERVO", mc_ip="192.168.100.100"):
        print("Failed to set action to McAction_RL_LOCOMOTION_ARM_EXT_JOINT_SERVO")
        return

    # Stops motion
    data = stop_motion_data()
    print(f"data: {data}")
    # Sends post request
    orin_ip = '192.168.100.100'

    response = requests.post(
        url='http://'+orin_ip +
            ':56444/rpc/aimdk.protocol.MotionCommandService/SendMotionCommand',
        headers={'content-type': 'application/json'},
        data=json.dumps(data,
                        ensure_ascii=False)  # Closes ascii for chinese
    )
    print(f"response: {response.text}")


def main(args=None):
    current_action = get_action(mc_ip="192.168.100.100")
    allowed_actions = [
        "McAction_RL_LOCOMOTION_DEFAULT",
        "McAction_RL_LOCOMOTION_ARM_EXT_JOINT_SERVO",
        "McAction_RL_NAVIGATION_DEFAULT",
    ]
    if current_action not in allowed_actions:
        print(
            f"Current action: {current_action} is not allowed, please set to one of the following: {allowed_actions}")
        return
    if not ensure_action("McAction_RL_LOCOMOTION_ARM_EXT_JOINT_SERVO", mc_ip="192.168.100.100"):
        print("Failed to set action to McAction_RL_LOCOMOTION_ARM_EXT_JOINT_SERVO")
        return

    # Stops motion
    data = stop_motion_data()
    print(f"data: {data}")
    # Sends post request
    orin_ip = '192.168.100.100'

    response = requests.post(
        url='http://'+orin_ip +
            ':56444/rpc/aimdk.protocol.MotionCommandService/SendMotionCommand',
        headers={'content-type': 'application/json'},
        data=json.dumps(data,
                        ensure_ascii=False)  # Closes ascii for chinese
    )
    print(f"response: {response.text}")

    rclpy.init(args=args)
    arm_publisher = ArmPublisher("/motion/control/arm_joint_command")
    arm_publisher.position_control()
    arm_publisher.position_recover()
    try:
        rclpy.spin(arm_publisher)
    except KeyboardInterrupt:
        pass
    finally:
        arm_publisher.destroy_node()
        rclpy.shutdown()


if __name__ == "__main__":
    main()
